#include "average_ground_seg.h"
namespace perception
{
    namespace lidar_preprocess
    {
        template <typename T1, typename T2, typename T3>
        void AverageGroundSeg<T1, T2, T3>::Process(typename pcl::PointCloud<T1>::Ptr &data_in,
                                                   typename pcl::PointCloud<T2>::Ptr &data_out, T3 &config)
        {
            if (data_in->points.empty())
            {
                std::cout << "ave ground seg no lidar points input\n";
                return;
            }
            typename pcl::PointCloud<T2>::Ptr no_ground_points(new pcl::PointCloud<T2>);
            typename pcl::PointCloud<T2>::Ptr ground_points(new pcl::PointCloud<T2>);
            std::vector<std::vector<grid_obj>> grid_preprocess;
            std::vector<cord> cord_size; // 存储地面点
            float grid_gradient_eps;
            float ground_height;
            float ground_height_eps = 0.1;
            int row;
            int col;
            int search_scope;
            // 1.计算栅格大小及初始化变量
            ground_points->points.clear();
            no_ground_points->points.clear();
            // grid_preprocess.clear();
            // 临时策略，最好还是在构造函数时进行初始化
            row = (int)(config.xmax - config.xmin) / config.cell_size_x;
            col = (int)(config.ymax - config.ymin) / config.cell_size_y;
            grid_preprocess = std::vector<std::vector<grid_obj>>(row, std::vector<grid_obj>(col));
            grid_gradient_eps = config.grid_gradient_eps;
            search_scope = config.search_scope;
            ground_height = config.ground_height;

            for (int i = 0; i < row; i++)
            {
                for (int j = 0; j < col; j++)
                {
                    grid_preprocess[i][j].reset();
                }
            }
            // Reset();
            // 2.得到每个栅格的最低值
            this->GetGridZmin(data_in, grid_preprocess, row, col, config);
            // 3.分割点云地面与非地面
            // this->ComputeGoundSeeds(grid_preprocess, cord_size, row, col, ground_height, ground_height_eps);
            // // 3.2.根据种子去找地面点
            // this->ComputeGound(grid_preprocess, cord_size, row, col, search_scope, grid_gradient_eps);
            // this->GetGroundPoints(data_in, grid_preprocess, ground_points, no_ground_points, row, col, config);
            this->GetGroundPointsOld(data_in, grid_preprocess, ground_points, no_ground_points, row, col, config);
            data_out->points.clear();
            data_out = no_ground_points;
            data_in->points.assign(ground_points->points.begin(), ground_points->points.end());
            // if (config.get_ground_points)
            //     data_out = ground_points;
            // else
            //     data_out = no_ground_points;
        }
        template <typename T1, typename T2, typename T3>
        void AverageGroundSeg<T1, T2, T3>::Process2(typename pcl::PointCloud<T1>::Ptr &data_in, typename pcl::PointCloud<T2>::Ptr &no_ground_points, typename pcl::PointCloud<T2>::Ptr &ground_points, T3 &config)
        {

            if (data_in->points.empty())
            {
                std::cout << "ave ground seg no lidar points input\n";
                return;
            }
            std::vector<std::vector<grid_obj>> grid_preprocess;
            std::vector<cord> cord_size; // 存储地面点
            float grid_gradient_eps;
            float ground_height;
            float ground_height_eps = 0.1;
            int row;
            int col;
            int search_scope;
            // 1.计算栅格大小及初始化变量
            // ground_points->points.clear();
            // no_ground_points->points.clear();
            // grid_preprocess.clear();
            // 临时策略，最好还是在构造函数时进行初始化

            row = (int)(config.xmax - config.xmin) / config.cell_size_x;
            col = (int)(config.ymax - config.ymin) / config.cell_size_y;
            grid_preprocess = std::vector<std::vector<grid_obj>>(row, std::vector<grid_obj>(col));
            grid_gradient_eps = config.grid_gradient_eps;
            search_scope = config.search_scope;
            ground_height = config.ground_height;

            for (int i = 0; i < row; i++)
            {
                for (int j = 0; j < col; j++)
                {
                    grid_preprocess[i][j].reset();
                }
            }

            // Reset();
            // 2.得到每个栅格的最低值
            this->GetGridZmin(data_in, grid_preprocess, row, col, config);
            // 3.分割点云地面与非地面
            this->ComputeGoundSeeds(grid_preprocess, cord_size, row, col, ground_height, ground_height_eps);
            // 3.2.根据种子去找地面点
            this->ComputeGound(grid_preprocess, cord_size, row, col, search_scope, grid_gradient_eps);
            this->GetGroundPoints(data_in, grid_preprocess, ground_points, no_ground_points, row, col, config);
        }
        template <typename T1, typename T2, typename T3>
        void AverageGroundSeg<T1, T2, T3>::ComputeGound(std::vector<std::vector<grid_obj>> &grid_preprocess, std::vector<cord> &cord_size,
                                                        int &row, int &col, float search_scope, float &grid_gradient_eps)
        {
            while (!cord_size.empty())
            {
                // WARN("cord_size1:{}", cord_size.size());
                auto next = cord_size[0];
                if (IsUsed(grid_preprocess, next.x, next.y))
                {
                    cord_size.erase(cord_size.begin());
                    continue;
                }
                grid_preprocess[next.x][next.y].is_used = true;
                ComputeGradient(grid_preprocess, cord_size, row, col, search_scope, grid_gradient_eps, next.x, next.y);
                cord_size.erase(cord_size.begin());
                // WARN("cord_size2:{}", cord_size.size());
            }
        }
        template <typename T1, typename T2, typename T3>
        inline bool AverageGroundSeg<T1, T2, T3>::IsSeed(std::vector<std::vector<grid_obj>> &grid_preprocess, int x, int y)
        {
            return grid_preprocess[x][y].is_seed;
        }
        template <typename T1, typename T2, typename T3>
        inline bool AverageGroundSeg<T1, T2, T3>::IsUsed(std::vector<std::vector<grid_obj>> &grid_preprocess, int x, int y)
        {
            return grid_preprocess[x][y].is_used;
        }
        template <typename T1, typename T2, typename T3>
        void AverageGroundSeg<T1, T2, T3>::ComputeGradient(std::vector<std::vector<grid_obj>> &grid_preprocess, std::vector<cord> &cord_size,
                                                           int &row, int &col, float search_scope, float &grid_gradient_eps, int x, int y)
        {
            for (int i = x - search_scope; i <= x + search_scope; i++)
            {
                for (int j = y - search_scope; j <= y + search_scope; j++)
                {
                    if (i < 0 || i >= row || j < 0 || j >= col || (i == x && j == y) || grid_preprocess[i][j].is_used)
                        continue;
                    float gradient = std::atan2(std::fabs(grid_preprocess[i][j].zmin - grid_preprocess[x][y].zmin),
                                                std::sqrt((i - x) * (i - x) + (j - y) * (j - y)));
                    if (gradient < grid_gradient_eps)
                    {
                        grid_preprocess[i][j].is_seed = true;
                        cord tmp;
                        tmp.x = i;
                        tmp.y = j;
                        cord_size.emplace_back(tmp);
                    }
                }
            }
        }
        template <typename T1, typename T2, typename T3>
        void AverageGroundSeg<T1, T2, T3>::ComputeGoundSeeds(std::vector<std::vector<grid_obj>> &grid_preprocess,
                                                             std::vector<cord> &cord_size, int &row, int &col, float &ground_height, float &ground_height_eps)
        {
            for (int i = 0; i < row; i++)
            {
                for (int j = 0; j < col; j++)
                {
                    // if (grid_preprocess[x][y].min < config.points_num) // 最小有效点云数
                    //     grid_preprocess[x][y].vaild = 0;
                    if (std::fabs(grid_preprocess[i][j].zmin - ground_height) < ground_height_eps)
                    {
                        grid_preprocess[i][j].is_seed = true;
                        cord tmp;
                        tmp.x = i;
                        tmp.y = j;
                        cord_size.emplace_back(tmp);
                    }
                }
            }
        }
        template <typename T1, typename T2, typename T3>
        int AverageGroundSeg<T1, T2, T3>::GetGridZmin(const typename pcl::PointCloud<T1>::Ptr &data_in,
                                                      std::vector<std::vector<grid_obj>> &grid_preprocess, int &row, int &col,
                                                      T3 &config)
        {
            if (data_in->empty())
            {
                // LOG(ERROR) << "no lidar data grond";
                return 1;
            }
            for (auto &p : data_in->points)
            {
                if (std::fabs(p.x > 50))
                    config.points_num = 1;
                else
                    config.points_num = 5;
                int x = int(std::floor(p.x - config.xmin) / config.cell_size_x);
                int y = int(std::floor(p.y - config.ymin) / config.cell_size_y);
                if (x > row - 1 || y > col - 1 || x < 0 || y < 0)
                    continue;
                Grid(grid_preprocess, row, x, y, p.z, config);
            }
            return 0;
        }
        template <typename T1, typename T2, typename T3>
        void AverageGroundSeg<T1, T2, T3>::Grid(std::vector<std::vector<grid_obj>> &grid_preprocess, int &row, int &x, int &y, float &z, T3 &config)
        {
            grid_preprocess[x][y].x = x * config.cell_size_x + config.xmin + config.cell_size_x / 2;
            grid_preprocess[x][y].y = y * config.cell_size_y + config.ymin + config.cell_size_y / 2;
            grid_preprocess[x][y].idx = x * row + y;
            if (grid_preprocess[x][y].point_num > 0)
            {
                // grid_preprocess[x][y].zmax = z > grid_preprocess[x][y].zmax ? z : grid_preprocess[x][y].zmax;
                grid_preprocess[x][y].zmin = z < grid_preprocess[x][y].zmin ? z : grid_preprocess[x][y].zmin;
            }
            else
            {
                // grid_preprocess[x][y].zmax = z;
                grid_preprocess[x][y].zmin = z;
            }
            grid_preprocess[x][y].point_num++;

            if (grid_preprocess[x][y].point_num > config.points_num) // 最小有效点云数
                grid_preprocess[x][y].vaild = 1;
        }
        template <typename T1, typename T2, typename T3>
        int AverageGroundSeg<T1, T2, T3>::GetGroundPoints(const typename pcl::PointCloud<T1>::Ptr &data_in,
                                                          std::vector<std::vector<grid_obj>> &grid_preprocess_,
                                                          typename pcl::PointCloud<T2>::Ptr &ground_points,
                                                          typename pcl::PointCloud<T2>::Ptr &no_ground_points,
                                                          int &row, int &col,
                                                          T3 &config)
        {
            typename pcl::PointCloud<T2>::Ptr cloud(new pcl::PointCloud<T2>);
            pcl::copyPointCloud(*data_in, *cloud);
            for (auto &p : cloud->points)
            {
                int x = int(std::floor(p.x - config.xmin) / config.cell_size_x);
                int y = int(std::floor(p.y - config.ymin) / config.cell_size_y);
                if (x > row - 1 || y > col - 1 || x < 0 || y < 0)
                    continue;
                if (grid_preprocess_[x][y].is_seed)
                {
                    if (p.z - grid_preprocess_[x][y].zmin > config.threshold_h)
                        no_ground_points->points.emplace_back(p);
                    else
                        ground_points->points.emplace_back(p);
                }
                else
                    no_ground_points->points.emplace_back(p);
            }
            return 0;
        }
        template <typename T1, typename T2, typename T3>
        int AverageGroundSeg<T1, T2, T3>::GetGroundPointsOld(const typename pcl::PointCloud<T1>::Ptr &data_in,
                                                             std::vector<std::vector<grid_obj>> &grid_preprocess_,
                                                             typename pcl::PointCloud<T2>::Ptr &ground_points,
                                                             typename pcl::PointCloud<T2>::Ptr &no_ground_points,
                                                             int &row, int &col,
                                                             T3 &config)
        {
            typename pcl::PointCloud<T2>::Ptr cloud(new pcl::PointCloud<T2>);
            pcl::copyPointCloud(*data_in, *cloud);
            for (auto &p : cloud->points)
            {
                int x = int(std::floor(p.x - config.xmin) / config.cell_size_x);
                int y = int(std::floor(p.y - config.ymin) / config.cell_size_y);
                config.threshold_h = 0.2 - std::fabs(p.x) * 0.002;
                // if (p.x > 50)
                // {
                //     config.threshold_h = -0.01;
                // }
                // else
                // {
                //     config.threshold_h = 0.2;
                // }
                if (x > row - 1 || y > col - 1 || x < 0 || y < 0)
                    continue;
                if (grid_preprocess_[x][y].vaild == 1)
                {
                    if (p.z - grid_preprocess_[x][y].zmin > config.threshold_h)
                        no_ground_points->points.emplace_back(p);
                    else
                        ground_points->points.emplace_back(p);
                }
                else
                    ground_points->points.emplace_back(p);
            }
            return 0;
        }
    }
}